#include <XnCppWrapper.h>
#include <cstdlib>
#include <cstdio>
#include <iostream>

#include <opencv2\core\core.hpp>
#include <opencv2\highgui\highgui.hpp> 
#include <opencv2\imgproc\imgproc.hpp>
#include <aruco.h>



using namespace xn;
using namespace std;
using namespace cv;
using namespace aruco;


//Mat depth;
//Mat rgb;
DepthGenerator dgen;
XnPoint3D corrispondenze[1024];

XnPoint3D getRelativeCoor(int x, int y, Mat depth) {
    XnPoint3D tobeconv;
    XnPoint3D real;
    tobeconv.X = x;
    tobeconv.Y = y;
    tobeconv.Z = depth.at<ushort > (y, x);
    dgen.ConvertProjectiveToRealWorld(1, &tobeconv, &real);
    return real;
}

int main(int argc, char* argv[]) {

    int key = 0;
    do {
        Context c;
        EnumerationErrors errors;
        ScriptNode sn;


        // generatore immagine profondita
        ImageGenerator igen; // generatore immagine rgb
        DepthMetaData dmd; // raccoglitore metadati (da cui prendere i pixel profondita)
        ImageMetaData imd; // raccoglitore metadati (da cui prendere i pixel rgb)
        XnMapOutputMode mapModeVGA;

        mapModeVGA.nXRes = 640;
        mapModeVGA.nYRes = 480;
        mapModeVGA.nFPS = 30;

        int nRetVal = c.InitFromXmlFile("config.xml", sn, &errors);
        nRetVal = c.FindExistingNode(XN_NODE_TYPE_DEPTH, dgen);
        nRetVal = c.FindExistingNode(XN_NODE_TYPE_IMAGE, igen);

        nRetVal = dgen.SetMapOutputMode(mapModeVGA);
        nRetVal = igen.SetMapOutputMode(mapModeVGA);
        nRetVal = dgen.GetAlternativeViewPointCap().SetViewPoint(igen);

        c.StartGeneratingAll();
        c.WaitAndUpdateAll();
        dgen.GetMetaData(dmd);
        igen.GetMetaData(imd);

        const XnDepthPixel* dpx = dmd.Data();
        const XnUInt8* ipx = imd.Data();


        Mat depth = Mat(480, 640, CV_16UC1, (char*) dpx);
        Mat rgb2 = Mat(480, 640, CV_8UC3, (char*) ipx);
        Mat rgb;
        cvtColor(rgb2, rgb, CV_RGB2BGR);
        /*rgb.create(480,640,CV_8UC3);
        depth.create(480,640,CV_16UC1);

        for(int i=0; i<640*480*3; i+=3){
                for(int j=0; j<3; j++){
                        rgb.at<Vec3b>((i/3)/640,(i/3)%640)[2-j] = ipx[i+j];
                }
        }

        for(int i=0; i<640*480; i++){
                depth.at<ushort>(i/640,i%640) = dpx[i];
        }*/

        MarkerDetector MDetector;
        vector<Marker> Markers;

        CameraParameters CamParam;
        CamParam.readFromXMLFile("camera.yml");

        //resizes the parameters to fit the size of the input image
        CamParam.resize(rgb.size());

        float MarkerSize = 0.205;

        //Ok, let's detect
        MDetector.detect(rgb, Markers, CamParam, MarkerSize);

        //for each marker, draw info and its boundaries in the image
        for (unsigned int i = 0; i < Markers.size(); i++) {
            cout << Markers[i] << endl;
            Markers[i].draw(rgb, Scalar(0, 0, 255), 2);
        }

        //draw a 3d cube in each marker if there is 3d info
        if (!CamParam.isValid()) {
            cout << "non e valido" << endl;
            return -1;
        }

        for (unsigned int i = 0; i < Markers.size(); i++) {
            CvDrawingUtils::draw3dCube(rgb, Markers[i], CamParam);
            cout << Markers[i].getCenter().x << " " << Markers[i].getCenter().y << endl;
        }

        for (unsigned int i = 0; i < Markers.size(); i++) {
            CvDrawingUtils::draw3dCube(rgb, Markers[i], CamParam);
            cout << "centro del marker " << Markers[i].getCenter() << endl;
            cout << "punti del marker " << Markers[i][0] << " - " << Markers[i][1] << " - "
                    << Markers[i][2] << " - " << Markers[i][3] << endl;
            XnPoint3D relative = getRelativeCoor(Markers[i].getCenter().x, Markers[i].getCenter().y, depth);
            XnPoint3D absolute;
            absolute.X = relative.X - corrispondenze[Markers[i].id].X;
            absolute.Y = relative.Y - corrispondenze[Markers[i].id].Y;
            absolute.Z = relative.Z - corrispondenze[Markers[i].id].Z;
        }
        //show input with augmented information
        imshow("in", rgb);

        key = cvWaitKey(1);
    } while (key != 'q');

    return 0;
}






